//
// Created by milk on 24-7-6.
//

#include "Quadrotor.h"

RegisterMSN *reg_pub = new RegisterMSN();
RegisterMSN *reg_sub = new RegisterMSN();

void PoseMsg::deserialize(const char *data, size_t length) {
    const char *ptr = data;
    size_t remaining = length;

    // 反序列化 seq
    if (remaining < sizeof(uint32_t)) throw std::runtime_error("Not enough data for seq size");
    uint32_t seq_size;
    memcpy(&seq_size, ptr, sizeof(seq_size));
    ptr += sizeof(seq_size);
    remaining -= sizeof(seq_size);

    if (seq_size > remaining) throw std::runtime_error("Not enough data for seq");
    seq.assign(ptr, ptr + seq_size);
    ptr += seq_size;
    remaining -= seq_size;

    // 反序列化 frame_id
    if (remaining < sizeof(uint32_t)) throw std::runtime_error("Not enough data for frame_id size");
    uint32_t frame_id_size;
    memcpy(&frame_id_size, ptr, sizeof(frame_id_size));
    ptr += sizeof(frame_id_size);
    remaining -= sizeof(frame_id_size);

    if (frame_id_size > remaining) throw std::runtime_error("Not enough data for frame_id");
    frame_id.assign(ptr, ptr + frame_id_size);
    ptr += frame_id_size;
    remaining -= frame_id_size;

    // 反序列化 xPoint
    if (remaining < sizeof(uint32_t)) throw std::runtime_error("Not enough data for xPoint size");
    uint32_t xPoint_size;

    memcpy(&xPoint_size, ptr, sizeof(xPoint_size));
    std::cout << xPoint_size << std::endl;
    ptr += sizeof(xPoint_size);
    remaining -= sizeof(xPoint_size);

    if (xPoint_size > remaining) throw std::runtime_error("Not enough data for xPoint");
    xPoint.assign(ptr, ptr + xPoint_size);
    ptr += xPoint_size;
    remaining -= xPoint_size;

    // 反序列化 yPoint
    if (remaining < sizeof(uint32_t)) throw std::runtime_error("Not enough data for yPoint size");
    uint32_t yPoint_size;
    memcpy(&yPoint_size, ptr, sizeof(yPoint_size));
    ptr += sizeof(yPoint_size);
    remaining -= sizeof(yPoint_size);

    if (yPoint_size > remaining) throw std::runtime_error("Not enough data for yPoint");
    yPoint.assign(ptr, ptr + yPoint_size);
    ptr += yPoint_size;
    remaining -= yPoint_size;

    // 反序列化 zPoint
    if (remaining < sizeof(uint32_t)) throw std::runtime_error("Not enough data for zPoint size");
    uint32_t zPoint_size;
    memcpy(&zPoint_size, ptr, sizeof(zPoint_size));
    ptr += sizeof(zPoint_size);
    remaining -= sizeof(zPoint_size);

    if (zPoint_size > remaining) throw std::runtime_error("Not enough data for zPoint");
    zPoint.assign(ptr, ptr + zPoint_size);
    ptr += zPoint_size;
    remaining -= zPoint_size;

    // 反序列化 xOrientation
    if (remaining < sizeof(uint32_t)) throw std::runtime_error("Not enough data for xOrientation size");
    uint32_t xOrientation_size;
    memcpy(&xOrientation_size, ptr, sizeof(xOrientation_size));
    ptr += sizeof(xOrientation_size);
    remaining -= sizeof(xOrientation_size);

    if (xOrientation_size > remaining) throw std::runtime_error("Not enough data for xOrientation");
    xOrientation.assign(ptr, ptr + xOrientation_size);
    ptr += xOrientation_size;
    remaining -= xOrientation_size;

    // 反序列化 yOrientation
    if (remaining < sizeof(uint32_t)) throw std::runtime_error("Not enough data for yOrientation size");
    uint32_t yOrientation_size;
    memcpy(&yOrientation_size, ptr, sizeof(yOrientation_size));
    ptr += sizeof(yOrientation_size);
    remaining -= sizeof(yOrientation_size);

    if (yOrientation_size > remaining) throw std::runtime_error("Not enough data for yOrientation");
    yOrientation.assign(ptr, ptr + yOrientation_size);
    ptr += yOrientation_size;
    remaining -= yOrientation_size;

    // 反序列化 zOrientation
    if (remaining < sizeof(uint32_t)) throw std::runtime_error("Not enough data for zOrientation size");
    uint32_t zOrientation_size;
    memcpy(&zOrientation_size, ptr, sizeof(zOrientation_size));
    ptr += sizeof(zOrientation_size);
    remaining -= sizeof(zOrientation_size);

    if (zOrientation_size > remaining) throw std::runtime_error("Not enough data for zOrientation");
    zOrientation.assign(ptr, ptr + zOrientation_size);
    ptr += zOrientation_size;
    remaining -= zOrientation_size;

    // 反序列化 wOrientation
    if (remaining < sizeof(uint32_t)) throw std::runtime_error("Not enough data for wOrientation size");
    uint32_t wOrientation_size;
    memcpy(&wOrientation_size, ptr, sizeof(wOrientation_size));
    ptr += sizeof(wOrientation_size);
    remaining -= sizeof(wOrientation_size);

    if (wOrientation_size > remaining) throw std::runtime_error("Not enough data for wOrientation");
    wOrientation.assign(ptr, ptr + wOrientation_size);
    ptr += wOrientation_size;
    remaining -= wOrientation_size;

    if (remaining != 0) throw std::runtime_error("Extra data at the end of the message");
}

std::vector<char> PoseMsg::serialize() const {
    std::vector<char> data;

    // 序列化 seq
    uint32_t seq_size = seq.size();
    data.insert(data.end(), reinterpret_cast<const char *>(&seq_size),
                reinterpret_cast<const char *>(&seq_size) + sizeof(seq_size));
    data.insert(data.end(), seq.begin(), seq.end());

    // 序列化 frame_id
    uint32_t frame_id_size = frame_id.size();
    data.insert(data.end(), reinterpret_cast<const char *>(&frame_id_size),
                reinterpret_cast<const char *>(&frame_id_size) + sizeof(frame_id_size));
    data.insert(data.end(), frame_id.begin(), frame_id.end());

    uint32_t xPoint_size = xPoint.size();
    data.insert(data.end(), reinterpret_cast<const char *>(&xPoint_size),
                reinterpret_cast<const char *>(&xPoint_size) + sizeof(xPoint_size));
    data.insert(data.end(), xPoint.begin(), xPoint.end());

    uint32_t yPoint_size = yPoint.size();
    data.insert(data.end(), reinterpret_cast<const char *>(&yPoint_size),
                reinterpret_cast<const char *>(&yPoint_size) + sizeof(yPoint_size));
    data.insert(data.end(), yPoint.begin(), yPoint.end());

    uint32_t zPoint_size = zPoint.size();
    data.insert(data.end(), reinterpret_cast<const char *>(&zPoint_size),
                reinterpret_cast<const char *>(&zPoint_size) + sizeof(zPoint_size));
    data.insert(data.end(), zPoint.begin(), zPoint.end());

    uint32_t xOrientation_size = xOrientation.size();
    data.insert(data.end(), reinterpret_cast<const char *>(&xOrientation_size),
                reinterpret_cast<const char *>(&xOrientation_size) + sizeof(xOrientation_size));
    data.insert(data.end(), xOrientation.begin(), xOrientation.end());

    uint32_t yOrientation_size = yOrientation.size();
    data.insert(data.end(), reinterpret_cast<const char *>(&yOrientation_size),
                reinterpret_cast<const char *>(&yOrientation_size) + sizeof(yOrientation_size));
    data.insert(data.end(), yOrientation.begin(), yOrientation.end());

    uint32_t zOrientation_size = zOrientation.size();
    data.insert(data.end(), reinterpret_cast<const char *>(&zOrientation_size),
                reinterpret_cast<const char *>(&zOrientation_size) + sizeof(zOrientation_size));
    data.insert(data.end(), zOrientation.begin(), zOrientation.end());

    uint32_t wOrientation_size = wOrientation.size();
    data.insert(data.end(), reinterpret_cast<const char *>(&wOrientation_size),
                reinterpret_cast<const char *>(&wOrientation_size) + sizeof(wOrientation_size));
    data.insert(data.end(), wOrientation.begin(), wOrientation.end());

    return data;
}

const char *Quadrotor::ipAddresses[] = {
        "192.168.1.201",
        "192.168.1.202",
        "192.168.1.203",
        "192.168.1.204",
        "192.168.1.205",
};

void Quadrotor::set_id(int id) {
    QuadrotorId = id;
}

int Quadrotor::get_id() {
    return QuadrotorId;
}

int Quadrotor::get_num() {
    return QuadrotorNum;
}

void Quadrotor::rosstateloopdata(const mavros_msgs::State::ConstPtr &state) {
    current_state = *state;
}

void Quadrotor::rosrcinloopdata(const mavros_msgs::RCIn::ConstPtr &RC_in) {
    channel5_value = RC_in->channels[4];
    channel6_value = RC_in->channels[5];
    channel7_value = RC_in->channels[6];
    channel8_value = RC_in->channels[7];

    

}

void Quadrotor::rosvelloopdata(const geometry_msgs::TwistStamped::ConstPtr &vel_local) {
    current_vel_x = vel_local->twist.linear.x;
    current_vel_y = vel_local->twist.linear.y;
    current_vel_z = vel_local->twist.linear.z;
}

void Quadrotor::rosaltitudeloopdata(const mavros_msgs::Altitude::ConstPtr &msg)
{
    current_point_z = msg->relative;
}

void Quadrotor::rosgpsloopdata(const sensor_msgs::NavSatFix::ConstPtr &gps) {
    self_gps_point.x = gps->longitude;
    self_gps_point.y = gps->latitude;
    self_gps_point.z = current_point_z;
}

